/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/








#include "btBulletDynamicsCommon.h"

#include <stdio.h> //printf debugging

#include "TrainSet.h"

//Needed around curves which are all perfect circles
const btScalar PI = (btScalar)3.14159265358979323846;


//Engine force applied only on front-most car
float	gEngineForce = 0.f;
float	maxEngineForce = 100000.f;  //this should actually be increased gradually and not all at once


//Braking force applied on all cars
float	gBrakingForce = 0.f;
float	maxBreakingForce = 500.f;  //again should be engaged gradually
float 	rollingFriction = 5000.;    //there should ideally be 2 friction values and it should be velocity dependent


//These are for use for diaplying wheels later(only eye candy, has no real effect on sim)
float	wheelRadius = 0.5f;
float	wheelHalfWidth = 0.2f;


//Coaches
float 	suspensionRestLength(2.);
float 	LINK_BREAKAGE_FORCE = 30000.;	//DO NOT accelerate suddenly, there will be car jerking
float 	K_UP   = 16000.;				//Above line suspender spring force constant
float 	K_YAW  = 100000.;				//On line trajectory corrector spring force constant
float	K_ROLL = 1000000.;				//Roll control, front wheel's segment's upaxis is used
float	K_LINK_FORCE = 100000.;


//Track
float railGap = 10.f;
float LINE_CHANGE_TOL = 0.2f;
TrackSegment *track1Head, *track2Head;
TrainCar car[MAX_CARS];		//This array must be global, if in class, crash!, probably due to alignment issues


//Platform
float platformHalfLength = 10.;


//Signal
float signalPoleHalfLength = 3.;
float signalGap = 2.5f;
float signalLampHeight = 0.3f;






////////////////////////////////////




Train::Train()
:
m_cameraHeight(4.f),
m_minCameraDistance(3.f),
m_maxCameraDistance(10.f)
{
	m_cameraPosition = btVector3(0,0,0);
	cameraEnabled = false;
	numSegsTrack1 = 0;
	reversed = false;
	
	
	//Track initialization
	track1Head = NULL;
	track2Head = NULL;

	createTrack1();
	
	createTrack2();

	createJunction1();
	createJunction2();


	//Rail infrastructure created from initPhysics() after dynamics world initialized

	//Train initialization-----------------

	for(int i=0; i<MAX_CARS; i++){
		car[i].segment[0] = track1Head;
		car[i].segment[1] = track1Head;
		car[i].prevDist[0] = 0.;
		car[i].prevDist[1] = 0.;
	}
	

	//Wheels
	connectionHeight = 0.5f;
	wheelFwdBackHalfDist = 8.f;
	wheelLeftRightHalfDist = 1.5f;
	chassisHeight = 1.f;


	//Car dimensions
	carHalfDimensions = btVector3(1.5f, 1.f, 9.f); //w,h,l = x,y,z
	linkLength = 1.f;

	//Platform dimensions
	platformHalfDimensions = btVector3(1.f, 1.f, 1.f);

	//Signal selection shape
	selectedShape = new btBoxShape(btVector3(1., 0.2f, 1.));
	currentSelTrkSeg = track1Head;
	currentSelSignal = 0;
	targetSpeed = 0.;

}

/**
 * Just basic init, more advanced stuff like platforms, signals etc set manually.
 */
void
Train::initializeSegment(TrackSegment* pTS, SegmentType segmentType, int segmentID)
{
	pTS->segmentType = segmentType;
	pTS->segmentID = segmentID;
	pTS->numSignals = 0;
	pTS->platformStart = NULL;
	pTS->platformEnd = NULL;

}

int
Train::createTrack1(void)
{
	//Initialize track segments : should check valid memory allocation
	TrackSegment *pTS = 0, *current = 0;	//to allow adding segments in between easily

	//Straight segment
	pTS = new TrackSegment(btVector3(-200.f, 0.f, 40.f), btVector3(1.f, 0.f, 0.f),
				btVector3(0.f, 1.f, 0.f), 200.f);
	initializeSegment(pTS, STRAIGHT, numSegsTrack1);
	current = pTS;
	current->next = NULL;
	current->prev = NULL;	
	track1Head = current;
	numSegsTrack1++;



	//circular arc segment
	pTS = new CircularArcSegment(btVector3(0.f, 0.f, 40.f), btVector3(0.f, 0.f, 1.f),
			btVector3(0.f, 1.f, 0.f),  40., SIMD_HALF_PI);
	initializeSegment(pTS, CIRCULAR, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;

	//Straight segment
	pTS = new TrackSegment(btVector3(40.f, 0.f, 0.f), btVector3(0.f, 0.f, -1.f),
				btVector3(0.f, 1.f, 0.f), 200.f);
	initializeSegment(pTS, STRAIGHT, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;

	//circular arc segment
	pTS = new CircularArcSegment(btVector3(40.f, 0.f, -200.f), btVector3(1., 0., 0.),
			btVector3(0.f, 1.f, 0.f),  40., SIMD_HALF_PI);
	initializeSegment(pTS, CIRCULAR, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;

	//Straight segment
	pTS = new TrackSegment(btVector3(0.f, 0.f, -240.f), btVector3(-1., 0., 0.),
				btVector3(0.f, 1.f, 0.f), 200.f);
	initializeSegment(pTS, STRAIGHT, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;

	//circular arc segment
	pTS = new CircularArcSegment(btVector3(-200., 0., -240.), btVector3(0., 0., -1.),
			btVector3(0.f, 1.f, 0.f),  40., SIMD_HALF_PI);
	initializeSegment(pTS, CIRCULAR, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;

	//Straight segment
	pTS = new TrackSegment(btVector3(-240., 0.f, -200.f), btVector3(0., 0., 1.),
				btVector3(0.f, 1.f, 0.f), 200.f);
	initializeSegment(pTS, STRAIGHT, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;


	//circular arc segment
	pTS = new CircularArcSegment(btVector3(-240., 0., 0.), btVector3(-1., 0., 0.),
			btVector3(0.f, 1.f, 0.f),  40., SIMD_HALF_PI);
	initializeSegment(pTS, CIRCULAR, numSegsTrack1);
	insertTrackSegment(current, pTS);
	current = current->next;	
	numSegsTrack1++;

	//Wrap track around, no null check to halt train yet
	current->next = track1Head;
	track1Head->prev = current;

	return 0;

}


int
Train::createTrack2(void)
{
	//Initialize track segments : should check valid memory allocation
	TrackSegment *pTS = 0, *current = 0;	//to allow adding segments in between easily

	//Straight segment
	pTS = new TrackSegment(btVector3(-200.f, 0.f, 50.f), btVector3(1.f, 0.f, 0.f),
				btVector3(0.f, 1.f, 0.f), 200.f);
	pTS->segmentType = STRAIGHT;
	current = pTS;
	current->next = NULL;
	current->prev = NULL;	
	track2Head = current;
	current->segmentID = 2;




	//circular arc segment
	pTS = new CircularArcSegment(btVector3(0.f, 0.f, 50.f), btVector3(0.f, 0.f, 1.f),
			btVector3(0.f, 1.f, 0.f),  50., SIMD_HALF_PI);
	pTS->segmentType = CIRCULAR;
	insertTrackSegment(current, pTS);
	current = current->next;
	

	//Straight segment
	pTS = new TrackSegment(btVector3(50.f, 0.f, 0.f), btVector3(0.f, 0.f, -1.f),
				btVector3(0.f, 1.f, 0.f), 200.f);
	pTS->segmentType = STRAIGHT;
	insertTrackSegment(current, pTS);
	current = current->next;	


	//circular arc segment
	pTS = new CircularArcSegment(btVector3(50.f, 0.f, -200.f), btVector3(1., 0., 0.),
			btVector3(0.f, 1.f, 0.f),  50., SIMD_HALF_PI);
	pTS->segmentType = CIRCULAR;
	insertTrackSegment(current, pTS);
	current = current->next;
	

	//Straight segment
	pTS = new TrackSegment(btVector3(0.f, 0.f, -250.f), btVector3(-1., 0., 0.),
				btVector3(0.f, 1.f, 0.f), 200.f);
	pTS->segmentType = STRAIGHT;
	insertTrackSegment(current, pTS);
	current = current->next;
	

	//circular arc segment
	pTS = new CircularArcSegment(btVector3(-200., 0., -250.), btVector3(0., 0., -1.),
			btVector3(0.f, 1.f, 0.f),  50., SIMD_HALF_PI);
	pTS->segmentType = CIRCULAR;
	insertTrackSegment(current, pTS);
	current = current->next;
	

	//Straight segment
	pTS = new TrackSegment(btVector3(-250., 0.f, -200.f), btVector3(0., 0., 1.),
				btVector3(0.f, 1.f, 0.f), 200.f);
	pTS->segmentType = STRAIGHT;
	insertTrackSegment(current, pTS);
	current = current->next;
	


	//circular arc segment
	pTS = new CircularArcSegment(btVector3(-250., 0., 0.), btVector3(-1., 0., 0.),
			btVector3(0.f, 1.f, 0.f),  50., SIMD_HALF_PI);
	pTS->segmentType = CIRCULAR;
	insertTrackSegment(current, pTS);
	current = current->next;


	//Wrap track around, no null check to halt train yet
	current->next = track2Head;
	track2Head->prev = current;


	numSegsTrack2 = numSegsTrack1;


	return 1;

}


/**
 * Currently number of segments are not specifically stored by track, so junction has just 1 segment
 * implicitly.
 */
int
Train::createJunction1(void)
{
	//Junction to move out from track1
	track1Head->outLength = 100.f;

	//Initialize track segments : should check valid memory allocation
	TrackSegment *pTS = 0, *current = 0;	//to allow adding segments in between easily


	btScalar r = 40.f;

	btScalar theta = btAcos((r-railGap/2)/r);

	btVector3 switchPoint = track1Head->origin;
	switchPoint += (track1Head->outLength * track1Head->forwardAxis);


	//1st circle segment to sway away from current track
	pTS = new CircularArcSegment( switchPoint, track1Head->rightAxis,
				btVector3(0.f, 1.f, 0.f),  r, -theta);
	pTS->segmentType = CIRCULAR;
	current = pTS;
	current->prev = track1Head;
	current->next = NULL;
	track1Head->outJunctionFirstSegment = current;
	current->segmentID = 3;
	



	//2nd circle segment to sway into target
	btVector3 rotDir = track1Head->rightAxis.rotate(track1Head->upAxis, -theta);
	//rotDir.normalize();

	switchPoint += track1Head->rightAxis * (-r); //move back to origin
	switchPoint += rotDir * r;  //move up to switch point

	pTS = new CircularArcSegment( switchPoint, -rotDir,
				btVector3(0.f, 1.f, 0.f),  r, theta);
	pTS->segmentType = CIRCULAR;
	insertTrackSegment(current, pTS);
	current = pTS;
	current->segmentID = 4;

	current->next = track2Head;
	track1Head->outJunctionLastSegment = current;

	track2Head->inJunctionLastSegment = current;

	//Calculating inLength
	switchPoint = current->origin + current->endAxis * current->length;
	switchPoint -= track2Head->origin;
	track2Head->inLength = switchPoint.dot(track2Head->forwardAxis);

	return 1;
}


/**
 * Currently number of segments are not specifically stored by track, so junction has just 1 segment
 * implicitly.
 */
int
Train::createJunction2(void)
{
	//Insert the switching point in the from track
	track2Head->outLength = 100.f;

	//Initialize track segments : should check valid memory allocation
	TrackSegment *pTS = 0, *current = 0;	//to allow adding segments in between easily


	btScalar r = 40.f;

	btScalar theta = btAcos((r-railGap/2)/r);

	btVector3 switchPoint = track2Head->origin;
	switchPoint += (track2Head->outLength * track2Head->forwardAxis);


	//1st circle segment to sway away from current track
	pTS = new CircularArcSegment( switchPoint, -track2Head->rightAxis,
				btVector3(0.f, 1.f, 0.f),  r, theta);
	pTS->segmentType = CIRCULAR;
	current = pTS;
	current->prev = track2Head;
	current->next = NULL;
	track2Head->outJunctionFirstSegment = current;
	current->segmentID = 5;




	//2nd circle segment to sway into target
	btVector3 rotDir = track2Head->rightAxis.rotate(track2Head->upAxis, theta);
	//rotDir.normalize();

	switchPoint += track2Head->rightAxis * (r);
	switchPoint += rotDir * (-r);

	pTS = new CircularArcSegment( switchPoint, rotDir,
				btVector3(0.f, 1.f, 0.f),  r, -theta);
	pTS->segmentType = CIRCULAR;
	insertTrackSegment(current, pTS);
	current = pTS;
	current->segmentID = 6;
	
	current->next = track1Head;
	track2Head->outJunctionLastSegment = current;

	track1Head->inJunctionLastSegment = current;

	//Calculating inLength
	switchPoint = current->origin + current->endAxis * current->length;
	switchPoint -= track1Head->origin;
	track1Head->inLength = switchPoint.dot(track1Head->forwardAxis);


	return 1;

}


/**
 * Create platforms and signals
 *
 * Platforms are made into rigid bodies to allow passengers to be rigid bodies.
 * Signals can be eye candy only for now.
 */
int
Train::createRailInfrastructure(void)
{
	btScalar coachSep = 2*carHalfDimensions.z() + 1.f;

	btTransform tr;
	tr.setIdentity();
	btQuaternion q;

	//Signal shapes
	signalPoleShape = new btCylinderShapeZ(btVector3(0.2f, 0.2f, signalPoleHalfLength));
	//m_collisionShapes.push_back(signalPoleShape);
	signalLampShape = new btSphereShape(0.3f);
	signalLampShape->setLocalScaling(btVector3(1., 0.5, 1.));



	//Platform shape
	btCompoundShape* platformShape = new btCompoundShape();
	m_collisionShapes.push_back(platformShape);


	//Platform floor
	btCollisionShape* floorShape = new btBoxShape(btVector3(2., 0.5, platformHalfLength));
	m_collisionShapes.push_back(floorShape);
	tr.setOrigin(btVector3(0., 0.5, 0.));
	platformShape->addChildShape(tr, floorShape);

	//Platform wall
	btCollisionShape* wallShape = new btBoxShape(btVector3(0.2f, 3., platformHalfLength));
	m_collisionShapes.push_back(wallShape);
	tr.setOrigin(btVector3(0., 4., 0.));
	platformShape->addChildShape(tr, wallShape);

	//Platform Ceiling
	btCollisionShape* ceilingShape = new btBoxShape(btVector3(10., 0.2f, platformHalfLength));
	m_collisionShapes.push_back(ceilingShape);
	tr.setOrigin(btVector3(0., 7.2f, 0.));
	platformShape->addChildShape(tr, ceilingShape);


	//------------------------------Track 1-----------------------------------

	//Draw entire track instead of just the active ones
	TrackSegment *i=track1Head;
	
	//Segment 1 signals
	i->signalDist[0] = 20.;
	i->numSignals = 1;

	//Segment 3 : Platform 1
    i = i->next;
    i = i->next;
	i->signalDist[0] = 30.;
	i->signalLamp[0] = OFF;
	i->numSignals = 1;
    i->platformStart = 100.;
    i->platformEnd =  i->platformStart + platformHalfLength;
	tr.setIdentity();
	tr.setOrigin( i->origin + btVector3(railGap/2, 0., 0.) + i->forwardAxis * (i->platformStart + platformHalfLength/2));
	localCreateRigidBody(0., tr, platformShape);

	//Segment 5 : Platform 2
	i = i->next;
    i = i->next;
    i->signalDist[0] = 30.;
    i->signalLamp[0] = OFF;
    i->numSignals = 1;
    i->platformStart = 100.;
    i->platformEnd =  i->platformStart + platformHalfLength;
	q.setEuler(PI/2, 0.f, 0.f);	//y,x,z
	tr.setIdentity();
	tr.setRotation(q);
	tr.setOrigin( i->origin + btVector3(0., 0., -railGap/2) + i->forwardAxis * (i->platformStart + platformHalfLength/2));
	localCreateRigidBody(0., tr, platformShape);

	//Segment 7 : Platform 3
	i = i->next;
    i = i->next;
    i->signalDist[0] = 30.;
    i->signalLamp[0] = OFF;
    i->numSignals = 1;
    i->platformStart = 100.;
    i->platformEnd =  i->platformStart + platformHalfLength;
	tr.setIdentity();
	tr.setOrigin( i->origin + btVector3(-railGap/2, 0., 0.) + i->forwardAxis * (i->platformStart + platformHalfLength/2));
	localCreateRigidBody(0., tr, platformShape);



	//------------------ Track 2 ----------------------------
	i = track2Head;

	//Segment 1 signals
	i->signalDist[0] = 20.;
	i->numSignals = 1;

	//Segment 3 : Platform 1
	i = i->next;
	i = i->next;
	i->signalDist[0] = 30.;
	i->signalLamp[0] = OFF;
	i->numSignals = 1;
//	i->platformStart = 100.;
//	i->platformEnd =  i->platformStart + platformHalfLength;
	tr.setIdentity();
	tr.setOrigin( i->origin + btVector3(railGap/2, 0., 0.) + i->forwardAxis * (i->platformStart + platformHalfLength/2));
//	localCreateRigidBody(0., tr, platformShape);

	//Segment 5 : Platform 2
	i = i->next;
	i = i->next;
	i->signalDist[0] = 30.;
	i->signalLamp[0] = OFF;
	i->numSignals = 1;
//	i->platformStart = 100.;
//	i->platformEnd =  i->platformStart + platformHalfLength;
	q.setEuler(PI/2, 0.f, 0.f);	//y,x,z
	tr.setIdentity();
	tr.setRotation(q);
	tr.setOrigin( i->origin + btVector3(0., 0., -railGap/2) + i->forwardAxis * (i->platformStart + platformHalfLength/2));
//	localCreateRigidBody(0., tr, platformShape);

	//Segment 7 : Platform 3
	i = i->next;
	i = i->next;
	i->signalDist[0] = 30.;
	i->signalLamp[0] = OFF;
	i->numSignals = 1;
//	i->platformStart = 100.;
//	i->platformEnd =  i->platformStart + platformHalfLength;
	tr.setIdentity();
	tr.setOrigin( i->origin + btVector3(-railGap/2, 0., 0.) + i->forwardAxis * (i->platformStart + platformHalfLength/2));
//	localCreateRigidBody(0., tr, platformShape);//Segment 1 signals



	return 1;
}

Train::~Train()
{
	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	

	//TODO: free the track segments
	//freeTracks();


	//delete dynamics world
	delete m_dynamicsWorld;


	//delete solver
	delete m_constraintSolver;

	//delete broadphase
	delete m_overlappingPairCache;

	//delete dispatcher
	delete m_dispatcher;

	delete m_collisionConfiguration;

}

void Train::initPhysics()
{
	//Normal Bullet initialization pattern
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	btVector3 worldMin(-1000.f, -1000.f, -1000.f);
	btVector3 worldMax(1000.f, 1000.f, 1000.f);
	m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
	m_constraintSolver = new btSequentialImpulseConstraintSolver();
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);


	//btCollisionShape* groundShape = new btBoxShape(btVector3(200,3,200));
	btCollisionShape *groundShape = new btStaticPlaneShape(btVector3(0, 1.f,0),0);
	m_collisionShapes.push_back(groundShape);

	//create ground object
	btTransform tr;
	tr.setIdentity();
	tr.setOrigin(btVector3(-120., -3.1f, -120.));
	//localCreateRigidBody(0.f ,tr,groundShape); //no ground as yet


	//Create train set
	defineCars();

	//Create rail infra
	createRailInfrastructure();

	focusCameraOnTrain();


}


/**
 * Defines the entire train set
 */
void
Train::defineCars(void)
{
	int i = 0; //, j;
	btScalar coachSep = 2*carHalfDimensions.z() + linkLength * 2;

	btTransform tr;
	tr.setIdentity();

	btCollisionShape* chassisShape = new btBoxShape(carHalfDimensions);
	m_collisionShapes.push_back(chassisShape);

	for(i=0; i<MAX_CARS; ++i){

		//car[i].origin = new btVector3(0., 0., 0.);

		//Initial position, on (0, initialHeight, 10) facing towards +ve x-axis
		btQuaternion q;
		q.setEuler(PI/2, 0.f, 0.f);	//y,x,z

		tr.setIdentity();
		tr.setRotation(q);
		car[i].origin = btVector3(-50 - coachSep * i, suspensionRestLength, 40.f);
		tr.setOrigin(car[i].origin);

		car[i].m_carChassis = localCreateRigidBody(800, tr, chassisShape);
		car[i].m_carChassis->setActivationState(DISABLE_DEACTIVATION);

		car[i].prevDist[0] = 0;
		car[i].prevDist[1] = 0;
		car[i].prevSegment[0] = 0;
		car[i].prevSegment[1] = 0;
	}




}


//to be implemented by the demo
void Train::renderme()
{
	int j;
	TrackSegment *i;
	btVector3 worldBoundsMin,worldBoundsMax;

	getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);

	updateCamera();
	

	//Draw entire track instead of just the active ones
	for(i=track1Head, j=0; i && j<numSegsTrack1; i = i->next, j++){
		glColor3f(0., 0., 0.);
		i->drawSegment();
		drawSignals(i, worldBoundsMin, worldBoundsMax);
	}



	//Draw selected shape
	btVector3 currentColor;
	btTransform tr;
	btScalar m[16];

	currentColor.setValue(1.f, 0.2f, 0.2f);
	tr.setIdentity();
	//tr.setRotation(q);
	btVector3 pos = currentSelTrkSeg->origin + btVector3(0., signalPoleHalfLength * 2.f + 2.f, 0.) -
		 (currentSelTrkSeg->rightAxis * signalGap) +
	     (currentSelTrkSeg->forwardAxis * currentSelTrkSeg->signalDist[currentSelSignal]);
	tr.setOrigin(pos);
	tr.getOpenGLMatrix(m);
	m_shapeDrawer->drawOpenGL(m, selectedShape, currentColor, getDebugMode(), worldBoundsMin, worldBoundsMax);


	//Draw entire track instead of just the active ones
	for(i=track2Head, j=0; i && j<numSegsTrack2; i = i->next, j++){
		glColor3f(0., 0., 0.);
		i->drawSegment();
		drawSignals(i, worldBoundsMin, worldBoundsMax);
	}



	glColor3f(1., 1., 1.);
	DemoApplication::renderme();

}



SIMD_FORCE_INLINE void
Train::drawSignals(TrackSegment *i, btVector3& worldBoundsMin, btVector3& worldBoundsMax)
{
	btVector3 currentColor(0.5,0.5,0.5), pos;
	btScalar m[16];
	btTransform tr;
	btQuaternion q(0., PI/2, 0.);	//y,x,z

	//Draw signals
	for(int k=0; k<i->numSignals; k++){

		//Pole
		currentColor.setValue(0.5f, 0.5, 0.5);
		tr.setIdentity();
		tr.setRotation(q);
		pos = i->origin + btVector3(0., signalPoleHalfLength, 0.) - (i->rightAxis * signalGap) + (i->forwardAxis * i->signalDist[k]);
		tr.setOrigin(pos);
		tr.getOpenGLMatrix(m);
		m_shapeDrawer->drawOpenGL(m,signalPoleShape,currentColor,getDebugMode(),worldBoundsMin,worldBoundsMax);


		currentColor.setValue(0.3f, 0, 0);
		if(i->signalLamp[k] == RED)
			currentColor.setValue(1, 0, 0);
		pos += btVector3(0., signalPoleHalfLength, 0.);
		tr.setOrigin(pos);
		tr.getOpenGLMatrix(m);
		m_shapeDrawer->drawOpenGL(m, signalLampShape,currentColor,getDebugMode(),worldBoundsMin,worldBoundsMax);

		currentColor.setValue(0.3f, 0.3f, 0);
		if(i->signalLamp[k] == YELLOW)
			currentColor.setValue(1, 1, 0);
		pos += btVector3(0., signalLampHeight, 0.);
		tr.setOrigin(pos);
		tr.getOpenGLMatrix(m);
		m_shapeDrawer->drawOpenGL(m, signalLampShape,currentColor,getDebugMode(),worldBoundsMin,worldBoundsMax);

		currentColor.setValue(0, 0.3f, 0);
		if(i->signalLamp[k] == GREEN)
			currentColor.setValue(0, 1, 0);
		pos += btVector3(0., signalLampHeight, 0.);
		tr.setOrigin(pos);
		tr.getOpenGLMatrix(m);
		m_shapeDrawer->drawOpenGL(m, signalLampShape,currentColor,getDebugMode(),worldBoundsMin,worldBoundsMax);

	}

}


void Train::clientMoveAndDisplay()
{

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 


	float dt = getDeltaTimeMicroseconds() * 0.000001f;
	
	updateCars(dt);
	



	if (m_dynamicsWorld)
	{
		//during idle mode, just run 1 simulation step maximum
		int maxSimSubSteps = m_idle ? 1 : 2;
		if (m_idle)
			dt = 1.0f/420.f;

		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
	}


	renderme(); 



	//optional but useful: debug drawing
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();


	glFlush();
	glutSwapBuffers();

}



void Train::displayCallback(void)
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	renderme();

//optional but useful: debug drawing
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	glutSwapBuffers();
}



void Train::clientResetScene()
{

}



void Train::specialKeyboardUp(int key, int x, int y)
{
   switch (key) 
    {
    case GLUT_KEY_UP :
		{
			gEngineForce = 0.f;
		break;
		}
	case GLUT_KEY_DOWN :
		{			
			gBrakingForce = 0.f;
		break;
		}
	default:
		DemoApplication::specialKeyboardUp(key,x,y);
        break;
    }

}


void Train::specialKeyboard(int key, int x, int y)
{


    switch (key) 
    {

    //Signalling keys
    case GLUT_KEY_LEFT : 
    	{
			--currentSelSignal;
			if(currentSelSignal < 0){
				do{
					currentSelTrkSeg = currentSelTrkSeg->prev;
				}while(currentSelTrkSeg->numSignals == 0); //if prev track has no signals goto track before that
				currentSelSignal = currentSelTrkSeg->numSignals-1;
			}

			focusCameraOnSignal();
			break;
		}

    case GLUT_KEY_RIGHT :
    	{
			++currentSelSignal;
			if(currentSelSignal == currentSelTrkSeg->numSignals){
				do{
					currentSelTrkSeg = currentSelTrkSeg->next;
				}while(currentSelTrkSeg->numSignals == 0); //if next track has no signals goto track after that
				currentSelSignal = 0;
			}

			focusCameraOnSignal();
			break;
		}

	case GLUT_KEY_PAGE_UP:
		currentSelTrkSeg->signalLamp[currentSelSignal] = SignalLampType((int(currentSelTrkSeg->signalLamp[currentSelSignal])+1) % 4);
		focusCameraOnSignal();
		break;


	//Engine keys
    case GLUT_KEY_UP :
		{
			if(reversed)
				gEngineForce = -maxEngineForce;
			else
				gEngineForce = maxEngineForce;

			gBrakingForce = 0.f;
			//focusCameraOnTrain();
			break;
		}
	case GLUT_KEY_DOWN :
		{			
			gBrakingForce = maxBreakingForce;
			gEngineForce = 0.f;
			//focusCameraOnTrain();
			break;
		}


	case GLUT_KEY_F7:
		currentSelTrkSeg = track1Head;
		focusCameraOnSignal();
		break;

	case GLUT_KEY_F8:
		currentSelTrkSeg = track2Head;
		focusCameraOnSignal();
		break;

	case GLUT_KEY_F9:
		reversed = !reversed;
		focusCameraOnTrain();
		break;


	//Camera follow
	case GLUT_KEY_F10:
		cameraEnabled = !cameraEnabled;
		focusCameraOnTrain();
		break;


	//Junction keys
	case GLUT_KEY_F11:
		track1Head->toggleJunction();
		m_cameraTargetPosition = track1Head->origin + (track1Head->forwardAxis * track1Head->outLength);
		break;


	case GLUT_KEY_F12:
		track2Head->toggleJunction();
		m_cameraTargetPosition = track2Head->origin + (track2Head->forwardAxis * track2Head->outLength);
		break;


	default:
		DemoApplication::specialKeyboard(key,x,y);
        break;
    }


}


void
Train::focusCameraOnSignal()
{
	m_cameraTargetPosition = currentSelTrkSeg->origin +
							(currentSelTrkSeg->forwardAxis * currentSelTrkSeg->signalDist[currentSelSignal]) +
							btVector3(0., signalPoleHalfLength, 0.) -
							(currentSelTrkSeg->rightAxis * signalGap);
}


void
Train::focusCameraOnTrain()
{
	btTransform chassisWorldTrans;

	//look at the vehicle
	car[0].m_carChassis->getMotionState()->getWorldTransform(chassisWorldTrans);
	m_cameraTargetPosition = chassisWorldTrans.getOrigin();
}

/**
 * Updates each car in the train set and interpolate their transforms for curving track
 * sections
 *
 * TODO: should also take speed and time delta into account for control or keep it simple, fast ?
 */
SIMD_FORCE_INLINE void
Train::updateCars(float dt)
{

	btMatrix3x3 mat;
	btMotionState *m;
	int i, j;

	car[0].m_carChassis->setActivationState(DISABLE_DEACTIVATION);

	btVector3 rearPoint, frontPoint, ztransprev;  //world space positions
	btScalar currentDist;


	for(i=0; i<MAX_CARS; ++i){

		m = car[i].m_carChassis->getMotionState();
		m->getWorldTransform(transCar);
		mat = transCar.getBasis();

		xtrans = mat.getColumn(0);
		ytrans = mat.getColumn(1);
		ztrans = mat.getColumn(2);
	
		dr  = transCar.getOrigin() - (car[i].origin);
		dv = dr/dt;
		car[i].origin = transCar.getOrigin();


		for(j=0; j<2; j++){  //0 is front wheel, 1 is back

			if(j==0)
				relPosWheel = ztrans * wheelFwdBackHalfDist;
			else
				relPosWheel = -ztrans * wheelFwdBackHalfDist;


			//Back wheel active segment based forces
			posWheel =  car[i].origin + relPosWheel;
			radiusWheel = posWheel - car[i].segment[j]->origin;


			/*
			 * The junction logic look ghastly but is actually quite straight forward. More details coming
			 * soon.
			 */
			switch(car[i].segment[j]->segmentType){

				case STRAIGHT:
				{
					wheelRightAxis = car[i].segment[j]->rightAxis;

					//Get projected position on FR plane of line: keep above line(vertical motion)
					projPlane =	radiusWheel -
										(radiusWheel.dot(car[i].segment[j]->upAxis) * car[i].segment[j]->upAxis);
					projPosFRPlane = projPlane + car[i].segment[j]->origin;

					//Get projected position on UF plane of line : keep on line(horizontal motion)
					projPlane =	radiusWheel -
										(radiusWheel.dot(wheelRightAxis) * wheelRightAxis);
					projPosUFPlane = projPlane + car[i].segment[j]->origin;

					//Straight segment handoffs : includes junction support
					currentDist = projPlane.dot(car[i].segment[j]->forwardAxis);
 
					if(car[i].segment[j]->outJunctionActive &&
					   car[i].prevSegment[j] == car[i].segment[j]->outJunctionFirstSegment->prev &&
					   car[i].prevDist[j] < car[i].segment[j]->outLength &&
					   currentDist > car[i].segment[j]->outLength)	//crossed out junction in last update, moving fwd
						car[i].segment[j] = car[i].segment[j]->outJunctionFirstSegment;
					else
					if(car[i].segment[j]->inJunctionActive &&
					   car[i].prevSegment[j] == car[i].segment[j]->inJunctionLastSegment->next &&
					   car[i].prevDist[j] > car[i].segment[j]->inLength &&
					   currentDist < car[i].segment[j]->inLength)	//crossed in junction in last update, moving back
					    car[i].segment[j] = car[i].segment[j]->inJunctionLastSegment;
					else
					if(currentDist > car[i].segment[j]->length)
						car[i].segment[j] = car[i].segment[j]->next;
					else
					if(currentDist < 0.)
						car[i].segment[j] = car[i].segment[j]->prev;


					
					car[i].prevDist[j] = currentDist;
					car[i].prevSegment[j] = car[i].segment[j];
					break;
				}


				case CIRCULAR:
				{
					wheelRightAxis = -radiusWheel.normalize();

					//Get projected position on FR plane of circle: keep above line(vertical motion)
					projPlane =	radiusWheel -
										(radiusWheel.dot(car[i].segment[j]->upAxis) * car[i].segment[j]->upAxis);
					projPosFRPlane = projPlane + car[i].segment[j]->origin;

					//Get projected position on UF plane at surface of cylinder : keep on line(horizontal motion)
					projPosUFPlane = (-wheelRightAxis)*car[i].segment[j]->length + car[i].segment[j]->origin;

					//Circular segment Handoff
					btScalar chkAngle = ( (car[i].segment[j]->angle < .0) ? -car[i].segment[j]->angle : car[i].segment[j]->angle);


					//Forward movement handoff 
					car[i].prevDist[j] = projPlane.angle(car[i].segment[j]->forwardAxis);
					if(car[i].prevDist[j] >= (chkAngle + SIMD_EPSILON)){ //angle() always returns +ve
						car[i].segment[j] = car[i].segment[j]->next;
					}	
					else{
						//Backward movement handoff
						car[i].prevDist[j] = projPlane.angle(car[i].segment[j]->endAxis);
						if(car[i].prevDist[j] >= (chkAngle + SIMD_EPSILON)){
							car[i].segment[j] = car[i].segment[j]->prev;
						}	
					}

			
					break;
				}


				default:
				;

			}//end switch





			force.setZero();

			//Keep car above line
			deviation = (suspensionRestLength*car[i].segment[j]->upAxis) - (posWheel - projPosFRPlane);
			force += (K_UP * deviation.dot(car[i].segment[j]->upAxis)) * car[i].segment[j]->upAxis;
			
	
			//Keep each wheel position on line(basically accomplishes yaw control)
			deviation = projPosUFPlane - posWheel;
			force += (K_YAW * deviation.dot(wheelRightAxis)) * wheelRightAxis;

			car[i].m_carChassis->applyForce(force, relPosWheel); //relative to car frame

			force.setZero();

			//Friction force, based on dr only , not dt or dv, the higher the dev. the higher the friction force
			//this will thus slow down car and ultimately be 0 itself when car halts, to prevent backward motion !!
			//Friction applies on both wheels
			float dev = dr.dot(ztrans);
			force -= ( (rollingFriction + 256*gBrakingForce) * dev) * ztrans;

			//Engine force on 1st car & front wheel only for now
			if(i==0 && j==0){
				force += gEngineForce * ztrans;
			}


			//Friction applies to everyone, engine force only to engine
			car[i].m_carChassis->applyCentralForce(force);



			//Back segment vectors
			/*glColor3f(0.f, 1.f, 1.f);
			drawVector(car[i].segment[j]->origin, car[i].origin);
			drawVector(car[i].segment[j]->origin, posWheel);
			drawVector(car[i].segment[j]->origin, projPosFRPlane);
			drawVector(car[i].segment[j]->origin, projPosUFPlane);*/

		}//for-j (front, back)




		force = K_ROLL * (ytrans.cross(car[i].segment[0]->upAxis));
		car[i].m_carChassis->applyTorque(force);


		//Link forces to push/pull the cars behind the engine
		if(i>0){
			frontPoint = car[i].origin + ztrans*(carHalfDimensions.z() + linkLength);

			btVector3 linkVector = rearPoint - frontPoint;


			btVector3 currentCarForce     = K_LINK_FORCE *  linkVector;
			btVector3 previousCarForce    = K_LINK_FORCE * -linkVector;  //force will be in opposite direction


			//Clamp this car's force to prevent forces at turns going out of control
			if(currentCarForce.x() > LINK_BREAKAGE_FORCE)
				currentCarForce.setX(LINK_BREAKAGE_FORCE);

			if(currentCarForce.x() < -LINK_BREAKAGE_FORCE)
				currentCarForce.setX(-LINK_BREAKAGE_FORCE);

			if(currentCarForce.y() > LINK_BREAKAGE_FORCE)
				currentCarForce.setY(LINK_BREAKAGE_FORCE);

			if(currentCarForce.y() < -LINK_BREAKAGE_FORCE)
				currentCarForce.setY(-LINK_BREAKAGE_FORCE);

			if(currentCarForce.z() > LINK_BREAKAGE_FORCE)
				currentCarForce.setZ(LINK_BREAKAGE_FORCE);

			if(currentCarForce.z() < -LINK_BREAKAGE_FORCE)
				currentCarForce.setZ(-LINK_BREAKAGE_FORCE);


			//Clamp previous car's force too for same reason
			if(previousCarForce.x() > LINK_BREAKAGE_FORCE)
				previousCarForce.setX(LINK_BREAKAGE_FORCE);

			if(previousCarForce.x() < -LINK_BREAKAGE_FORCE)
				previousCarForce.setX(-LINK_BREAKAGE_FORCE);

			if(previousCarForce.y() > LINK_BREAKAGE_FORCE)
				previousCarForce.setY(LINK_BREAKAGE_FORCE);

			if(previousCarForce.y() < -LINK_BREAKAGE_FORCE)
				previousCarForce.setY(-LINK_BREAKAGE_FORCE);

			if(previousCarForce.z() > LINK_BREAKAGE_FORCE)
				previousCarForce.setZ(LINK_BREAKAGE_FORCE);

			if(previousCarForce.z() < -LINK_BREAKAGE_FORCE)
				previousCarForce.setZ(-LINK_BREAKAGE_FORCE);


			//Force on this car due to this link
			car[i].m_carChassis->applyForce(currentCarForce, ztrans*(carHalfDimensions.z() + linkLength)); //relative to car frame

			//Force on car infront of this car
			car[i-1].m_carChassis->applyForce(previousCarForce, (-ztransprev)*(carHalfDimensions.z() + linkLength)); //relative to car frame

		}

		//Keep current cars rear point for next car's calculation
		rearPoint = car[i].origin - ztrans*(carHalfDimensions.z() + linkLength);

		//The z-transform as well
		ztransprev = ztrans;


	}//end for-i...MAX_CARS



	cabControl(ztrans);

}


int
Train::cabControl(btVector3& ztrans)
{
	//-------------------------- Cab control(only forward movement) -----------------------------
	//TODO: Back cab port of logic for backward movement
	SignalLampType engineReceivedSignal = OFF;
	TrackSegment *sigChkSeg;
		
	//Check if engine ahead of last signal for current segment of engine
	if(car[0].prevDist[0] > car[0].segment[0]->signalDist[car[0].segment[0]->numSignals-1]){
		sigChkSeg = car[0].segment[0];
		do{
			sigChkSeg = sigChkSeg->next;
		}while(sigChkSeg->numSignals == 0); //this will loop infinitely if entire track has no signals : must set visibility limits
		engineReceivedSignal = sigChkSeg->signalLamp[0]; //set to lamp of 1st sig in a segment that has signals after current
	}
	else{
		//Nope, there is a signal in this segment ahead of the cab which must be found
		for(int i=0; i<car[0].segment[0]->numSignals; i++){ //start from track origin to allow simple dist check
			if(car[0].prevDist[0] < car[0].segment[0]->signalDist[i])
				engineReceivedSignal = car[0].segment[0]->signalLamp[i]; //set to lamp of 1st sig < cab dist
		}
	}

	speed = dv.dot(ztrans);
	float dTargetSpeed = 0.;  	
	
	//If signal off then targetSpeed unchanged, cab controller continues to try and achieve it
	switch(engineReceivedSignal){
	case RED:		
		targetSpeed =  0.;
		break;
	case YELLOW:		
		targetSpeed = MAX_YELLOW_SPEED;
		break;
	case GREEN:		
		targetSpeed = MAX_GREEN_SPEED;
		break;
	}

	float ke = 100.;	
	dTargetSpeed = targetSpeed - speed;
	gEngineForce = dTargetSpeed * ke; //force reduced as target speed approached(like a spring)

	//printf("sp:%f, dTsp:%f, gEngForce:%f\n", speed, dTargetSpeed, gEngineForce);

	return 0;
}


int
Train::deleteTrackSegment(TrackSegment *pTS)
{

	//Check for empty list
	if(pTS == NULL)
		return 0;



	if(pTS->prev)
		pTS->prev->next = pTS->next;

	if(pTS->next)
		pTS->next->prev = pTS->prev;



	return 1;
}


/**
 * Inserts at the next pointer, so after the reference node, useful while creating track.
 * Does not allocate memory in any way. ref must not be NULL
 */
int
Train::insertTrackSegment(TrackSegment *ref, TrackSegment *pTS)
{
	//Check for empty list
	if(pTS == NULL)
		return 0;

	//1st node cant be added here, ptr to ptr has to be passed to modify calling func copy

	//if inserting in middle, then ref->next should be copied to pTS->next, else no(when connecting
	//first & last node(ref=last, pTS=first) first->next will be nulled, losing ptr to second node!)
	if(ref->next)
		ref->next->prev = pTS;

	pTS->next = ref->next; //if this is inside above if, then NULL not passed fwd during list constr
	pTS->prev = ref;
	ref->next = pTS;

	return 1;
}


/**
 * Update camera
 */
void	
Train::updateCamera()
{
	
	if(!cameraEnabled){
		DemoApplication::updateCamera();
		return;
	}


	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();


	//m_cameraTargetPosition set in keyboard function
	btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;

	//keep distance between min and max distance
	float cameraDistance = camToObject.length();
	float correctionFactor = 0.f;
	if (cameraDistance < m_minCameraDistance)
	{
		correctionFactor = 0.15f*(m_minCameraDistance-cameraDistance)/cameraDistance;
	}
	if (cameraDistance > m_maxCameraDistance)
	{
		correctionFactor = 0.15f*(m_maxCameraDistance-cameraDistance)/cameraDistance;
	}
	m_cameraPosition -= correctionFactor*camToObject;
	
	  btScalar aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
        glFrustum (-aspect, aspect, -1.0f, 1.0f, 1.0f, 10000.0f);

         glMatrixMode(GL_MODELVIEW);
         glLoadIdentity();

    gluLookAt(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2],
                      m_cameraTargetPosition[0],m_cameraTargetPosition[1], m_cameraTargetPosition[2],
                          m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());



}





/**
 * Draw a vector, color set by caller
 */
void
drawVector(btVector3& from, btVector3& to)
{
	glBegin(GL_LINES);
		glVertex3f(BT3ARGS(from));
		glVertex3f(BT3ARGS(to));
	glEnd();
}

